{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# In this example we will show the difference between fixes computed with laika\n",
    "# from raw data of the ublox receiver vs the the fixes the ublox receiver\n",
    "# computes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pathlib import Path\n",
    "\n",
    "base_dir = Path('example_data')\n",
    "raw_ublox_t = np.load(base_dir / 'raw_gnss_ublox/t')\n",
    "raw_ublox = np.load(base_dir / 'raw_gnss_ublox/value')\n",
    "fixes_ublox_t = np.load(base_dir / 'live_gnss_ublox/t')\n",
    "fixes_ublox = np.load(base_dir / 'live_gnss_ublox/value')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We get the raw data into our format from the log array format\n",
    "\n",
    "from laika.raw_gnss import normal_meas_from_array\n",
    "\n",
    "measurements = np.array([normal_meas_from_array(arr) for arr in raw_ublox])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# initialize an astrodog with dgps corrections\n",
    "\n",
    "from laika import AstroDog\n",
    "\n",
    "dog = AstroDog(dgps=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "PosixPath('/tmp/gnss/cors_coord/cors_station_positions')"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Building this cache takes forever, just copy it from repo\n",
    "\n",
    "from shutil import copyfile\n",
    "\n",
    "cache_directory = Path(dog.cache_dir) / 'cors_coord'\n",
    "cache_directory.mkdir(parents=True, exist_ok=True)\n",
    "copyfile('cors_station_positions', cache_directory / 'cors_station_positions')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Downloading https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/18213/final/Sta20123.sp3\n",
      "Downloading https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/18214/final/Sta20124.sp3\n",
      "Downloading https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/18215/final/Sta20125.sp3\n",
      "Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/2012/igs20123.sp3.Z\n",
      "Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/2012/igs20124.sp3.Z\n",
      "Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/2012/igs20125.sp3.Z\n"
     ]
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "7e625de4e34e44f0922ead82c10c26e0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "  0%|          | 0/600 [00:00<?, ?it/s]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/pbl1/pbl12140.18d.gz\n",
      "HTTPS error 404\n",
      "Downloading https://alt.ngs.noaa.gov/corsdata/rinex/2018/214/pbl1/pbl12140.18d.gz\n",
      "HTTPS error 404\n",
      "File not downloaded, check availability on server.\n",
      "Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/pbl2/pbl22140.18d.gz\n",
      "HTTPS error 404\n",
      "Downloading https://alt.ngs.noaa.gov/corsdata/rinex/2018/214/pbl2/pbl22140.18d.gz\n",
      "HTTPS error 404\n",
      "File not downloaded, check availability on server.\n",
      "Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/hsib/hsib2140.18d.gz\n",
      "HTTPS error 404\n",
      "Downloading https://alt.ngs.noaa.gov/corsdata/rinex/2018/214/hsib/hsib2140.18d.gz\n",
      "HTTPS error 404\n",
      "File not downloaded, check availability on server.\n",
      "Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/tibb/tibb2140.18d.gz\n",
      "Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/capo/capo2140.18d.gz\n",
      "Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/ionex/2018/214/codg2140.18i.Z\n",
      "Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/bias/2018/CAS0MGXRAP_20182140000_01D_01D_DCB.BSX.gz\n"
     ]
    }
   ],
   "source": [
    "from laika.raw_gnss import process_measurements, correct_measurements, calc_pos_fix\n",
    "from tqdm.auto import tqdm\n",
    "\n",
    "# We want to group measurements by measurement epoch\n",
    "# this makes the kalman filter faster and is easier\n",
    "# to reason about\n",
    "grouped_t = np.unique(raw_ublox_t)\n",
    "grouped_meas_processed = []\n",
    "corrected_meas_arrays = []\n",
    "\n",
    "# process measurement groups\n",
    "for t in grouped_t:\n",
    "    meas = measurements[raw_ublox_t == t]\n",
    "    grouped_meas_processed.append(process_measurements(meas, dog))\n",
    "\n",
    "# correct measurement groups with an estimate position\n",
    "# that was computes with weighted-least-squares on\n",
    "# the first epoch\n",
    "# WARNING: can take up to 10min\n",
    "wls_estimate = calc_pos_fix(grouped_meas_processed[0])\n",
    "est_pos = wls_estimate[0][:3]\n",
    "for proc in tqdm(grouped_meas_processed):\n",
    "    corrected = correct_measurements(proc, est_pos, dog)\n",
    "    corrected_meas_arrays.append(np.array([c.as_array() for c in corrected]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "8b1fa61873754be19c7f3e9d32d880f8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "  0%|          | 0/600 [00:00<?, ?it/s]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "for proc in tqdm(grouped_meas_processed):\n",
    "    corrected = correct_measurements(proc, est_pos, dog)\n",
    "    corrected_meas_arrays.append(np.array([c.as_array() for c in corrected]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Generate and build a CFFI library from the Kalman filter defined symbolically in SymPy\n",
    "from kalman.models.gnss_kf import GNSSKalman\n",
    "\n",
    "GNSSKalman.generate_code()\n",
    "!g++ kalman/generated/gnss.cpp -Wall -fPIC -shared -o kalman/generated/libgnss.so"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "473c7197f3f84f01b41d672c5c030eba",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "  0%|          | 0/1200 [00:00<?, ?it/s]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# We run the Kalman filter\n",
    "\n",
    "from kalman.kalman_helpers import run_car_ekf_offline, ObservationKind\n",
    "\n",
    "ekf = GNSSKalman()\n",
    "init_state = ekf.x\n",
    "init_state[:3] = est_pos\n",
    "ekf.init_state(init_state)\n",
    "ekf_data = {}\n",
    "ekf_data[ObservationKind.PSEUDORANGE_GPS] = (grouped_t, corrected_meas_arrays)\n",
    "ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = (grouped_t, corrected_meas_arrays)\n",
    "ekf_outputs = run_car_ekf_offline(ekf, ekf_data)\n",
    "\n",
    "import laika.lib.coordinates as coord\n",
    "\n",
    "laika_positions_t = ekf_outputs[4]\n",
    "laika_positions_ecef = ekf_outputs[0][:, :3]\n",
    "laika_positions_geodetic = coord.ecef2geodetic(laika_positions_ecef)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "ublox_positions_geodetic = fixes_ublox[:, [0, 1, 4]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "# By looking at the map, we can see that the two paths compared.\n",
    "# If you want to regenerate the gmplot you will need a google\n",
    "# maps API key\n",
    "\n",
    "import gmplot\n",
    "\n",
    "gmap = gmplot.GoogleMapPlotter(*laika_positions_geodetic[0])\n",
    "#gmap.apikey='...'\n",
    "gmap.plot([x[0] for x in laika_positions_geodetic], [x[1] for x in laika_positions_geodetic], 'blue', edge_width=5)\n",
    "gmap.plot([x[0] for x in ublox_positions_geodetic], [x[1] for x in ublox_positions_geodetic], 'red', edge_width=5)\n",
    "gmap.draw(\"laika_quality_check.html\")\n",
    "\n",
    "import webbrowser\n",
    "import os\n",
    "\n",
    "webbrowser.open('file://' + os.path.realpath(\"laika_quality_check.html\"));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}